#ifndef _FLOORPLAN_H_
#define _FLOORPLAN_H_

#include <Stage-4.1/stage.hh>

#include <vector>
#include <string>

class Floorplan : public Stg::Model
{

public:

	Floorplan(Stg::World *world, std::string fname, double width, double height, double wall_height) : Stg::Model(world, NULL, "floorplan")
	{
		SetObstacleReturn(true);
		SetRangerReturn(1.0);
		SetGuiMove(false);
		SetGuiOutline(true);
		SetColor(Stg::Color("grey30"));
		SetBoundary(true);

		// set size of floorplan
		Stg::Geom g;
		g.size = Stg::Size(width, height, wall_height);
		SetGeom(g);

		load_bitmap(fname);

		Subscribe(); Startup();
	}

	virtual ~Floorplan()
	{}

	virtual void DrawBlocks()
	{
		glEnable(GL_COLOR_MATERIAL);
		Stg::Model::DrawBlocks();
	}

	void load_bitmap(std::string fname)
	{
		// load bitmap from file and turn into blocks
		std::vector<Stg::rotrect_t> rects;
		if(Stg::rotrects_from_image_file(fname, rects))
		{
			printf("failed to load rects from image file \"%s\"", fname.c_str());
			return;
		}

		float minx = Stg::billion;
		float miny = Stg::billion;
		float maxx = -Stg::billion;
		float maxy = -Stg::billion;

		FOR_EACH(rect, rects)
		{
			std::vector<Stg::point_t> pts(4);

			const double x = rect->pose.x;
			const double y = rect->pose.y;
			const double w = rect->size.x;
			const double h = rect->size.y;

			pts[0].x = x;
			pts[0].y = y;
			pts[1].x = x + w;
			pts[1].y = y;
			pts[2].x = x + w;
			pts[2].y = y + h;
			pts[3].x = x;
			pts[3].y = y + h;

			if (x < minx) minx = x;
			if (y < miny) miny = y;
			if (x+w > maxx) maxx = x+w;
			if (y+h > maxy) maxy = y+h;

			blockgroup.AppendBlock(new Stg::Block(this, pts, 0, 1, color, true, false));
		}

		blockgroup.CalcSize();
		double epsilon = 0.01;
		Stg::Size bgsize = blockgroup.GetSize();
		AddBlockRect(minx, miny, epsilon, bgsize.y, bgsize.z );
		AddBlockRect(minx, miny, bgsize.x, epsilon, bgsize.z );
		AddBlockRect(minx, maxy-epsilon, bgsize.x, epsilon, bgsize.z );
		AddBlockRect(maxx-epsilon, miny, epsilon, bgsize.y, bgsize.z );

		blockgroup.CalcSize();

		UnMapWithChildren(0);
		UnMapWithChildren(1);
		MapWithChildren(0);
		MapWithChildren(1);
	}

private:

};

#endif
